/**
* This file is part of MultiCol-SLAM
*
* Copyright (C) 2015-2016 Steffen Urban <urbste at googlemail.com>
* For more information see <https://github.com/urbste/MultiCol-SLAM>
*
* MultiCol-SLAM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* MultiCol-SLAM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with MultiCol-SLAM . If not, see <http://www.gnu.org/licenses/>.
*/
#include "cam_system_omni.h"
#include "cConverter.h"

namespace MultiColSLAM
{
	void cMultiCamSys_::WorldToCam(int c,
		cv::Point3_<double>& pt3,
		cv::Point_<double>& pt2)
	{
		cv::Matx<double, 4, 4> M_ = M_t*M_c[c];

		cv::Matx<double, 4, 1> pt3_(pt3.x, pt3.y, pt3.z, 1.0);

		cv::Matx<double, 4, 1> ptRot = cConverter::invMat(M_)*pt3_;
		pt2.x = 0.0;
		pt2.y = 0.0;

		camModels[c].WorldToImg(
			ptRot(0, 0), ptRot(1, 0), ptRot(2, 0),
			pt2.x, pt2.y);
	}

	/**
	 * @brief 将世界坐标系中的点转换到相机坐标系中
	 *
	 * 将给定的三维点从世界坐标系转换到指定的相机坐标系中，并返回相应的二维点。
	 *
	 * @param c 指定相机的索引
	 * @param pt3 世界坐标系中的三维点
	 * @param pt2 存储转换后的二维点的变量
	 */
	void cMultiCamSys_::WorldToCam(int c,
		cv::Point3_<double>& pt3,
		cv::Vec2d& pt2)
	{
		// 创建一个4x1的矩阵，用于存储旋转后的点坐标
		cv::Matx<double, 4, 1> ptRot;
		// 将3D点坐标转换为4x1矩阵形式
		cv::Matx<double, 4, 1> pt3_(pt3.x, pt3.y, pt3.z, 1.0);

		// 判断是否进行了标记矩阵的变换
		if (!flagMcMt)
		{
			// 计算变换矩阵M_
			cv::Matx<double, 4, 4> M_ = M_t * M_c[c];
			// 计算旋转后的点坐标
			ptRot = cConverter::invMat(M_)*pt3_;
		}
		else
		{
			// 使用已经计算好的逆矩阵进行变换
			ptRot = MtMc_inv[c] * pt3_;
		}

		// 初始化pt2为0.0
		pt2 = 0.0;
		// 再次初始化pt2为0.0（这一行是多余的，可能是代码冗余）
		pt2 = 0.0;

		// 将旋转后的4x1矩阵转换为3D点坐标
		cv::Point3_<double> ptRot3;
		ptRot3.x = ptRot(0, 0);
		ptRot3.y = ptRot(1, 0);
		ptRot3.z = ptRot(2, 0);

		// 调用相机模型的WorldToImg方法，将3D点坐标转换为2D图像坐标
		camModels[c].WorldToImg(ptRot3.x, ptRot3.y, ptRot3.z, pt2(0), pt2(1));
	}

	void cMultiCamSys_::WorldToCamHom(int c,
		cv::Vec<double, 4>& pt4,
		cv::Vec2d& pt2)
	{
		cv::Matx<double, 4, 1> ptRot;
		if (!flagMcMt)
		{
			cv::Matx<double, 4, 4> M_ = M_t * M_c[c];
			ptRot = cConverter::invMat(M_)*pt4;
		}
		else
			ptRot = MtMc_inv[c] * pt4;

		pt2 = 0.0;
		pt2 = 0.0;

		cv::Point3_<double> ptRot3;
		ptRot3.x = ptRot(0, 0);
		ptRot3.y = ptRot(1, 0);
		ptRot3.z = ptRot(2, 0);

		camModels[c].WorldToImg(ptRot(0, 0), ptRot(1, 0), ptRot(2, 0),
			pt2(0), pt2(1));
	}

	bool cMultiCamSys_::WorldToCamHom_fast(int c,
		cv::Vec<double, 4>& pt4,
		cv::Vec<double, 2>& pt2)
	{
		cv::Matx<double, 4, 1> ptRot;
		if (!flagMcMt)
		{
			cv::Matx<double, 4, 4> M_ = M_t * M_c[c];

			ptRot = cConverter::invMat(M_) * pt4;
		}
		else
			ptRot = MtMc_inv[c] * pt4;

		pt2(0) = 0.0;
		pt2(1) = 0.0;

		camModels[c].WorldToImg(ptRot(0, 0), ptRot(1, 0), ptRot(2, 0),
			pt2(0), pt2(1));
		return ptRot(2, 0) <= 0.0;
	}

	void cMultiCamSys_::WorldToCamHom_fast(int c,
		cv::Vec<double, 3>& pt3,
		cv::Vec<double, 2>& pt2)
	{
		cv::Matx<double, 4, 1> ptRot;
		if (!flagMcMt)
		{
			cv::Matx<double, 4, 4> M_ = M_t * M_c[c];

			ptRot = cConverter::invMat(M_) * cv::Vec4d(pt3(0), pt3(1), pt3(2), 1.0);
		}
		else
			ptRot = MtMc_inv[c] * cv::Vec4d(pt3(0), pt3(1), pt3(2), 1.0);

		pt2(0) = 0.0;
		pt2(1) = 0.0;

		camModels[c].WorldToImg(ptRot(0, 0), ptRot(1, 0), ptRot(2, 0),
			pt2(0), pt2(1));
	}

	void cMultiCamSys_::CamToWorld_ogv(int c,
		opengv::bearingVector_t& bearingV,
		cv::Point_<double> pt2)
	{
		cv::Point3_<double> pt3;
		camModels[c].ImgToWorld(pt3,
			pt2);

		bearingV[0] = pt3.x;
		bearingV[1] = pt3.y;
		bearingV[2] = pt3.z;

	}

	void cMultiCamSys_::CamToWorld(int c, cv::Point3_<double>& pt3, cv::Point_<double>& pt2)
	{
		// 定义一个临时变量pt3t，用于存储转换后的世界坐标
		cv::Point3_<double> pt3t;
		// 调用相机模型的ImgToWorld方法，将图像坐标pt2转换为世界坐标pt3t
		camModels[c].ImgToWorld(pt3t, pt2);

		// 将转换后的世界坐标pt3t赋值给输出参数pt3
		pt3.x = pt3t.x;
		pt3.y = pt3t.y;
		pt3.z = pt3t.z;
	}

	void cMultiCamSys_::CamToWorld(int c, cv::Vec<double, 3>& pt3, cv::Point_<double>& pt2)
	{
	    // 定义一个三维点
		cv::Point3_<double> pt3t;

	    // 将图像中的二维点转换为世界坐标系中的三维点
	    // 使用相机模型中的ImgToWorld方法
		camModels[c].ImgToWorld(pt3t, pt2);

	    // 将转换后的三维点赋值给输入的三维向量
	    // X轴分量
		pt3(0) = pt3t.x;
	    // Y轴分量
		pt3(1) = pt3t.y;
	    // Z轴分量
		pt3(2) = pt3t.z;
	}

	// set pose
	void cMultiCamSys_::Set_M_t_from_min(cv::Matx<double, 6, 1> M_t_minRep)
	{
		// 将传入的 M_t_minRep 赋值给 M_t_min
		M_t_min = M_t_minRep;
		// 将 M_t_minRep 转换为齐次坐标矩阵并赋值给 M_t
		M_t = cayley2hom<double>(M_t_minRep);
		// 注释掉的其他转换方式
		//M_t = rodrigues2hom<double>(M_t_minRep);
		// 计算 M_t 的逆矩阵并赋值给 M_t_inv
		M_t_inv = cConverter::invMat(M_t);

		// 遍历所有相机
		for (int c = 0; c < nrCams; ++c)
		{
			// 计算当前相机在全局坐标系下的位置矩阵并赋值给 MtMc[c]
			MtMc[c] = M_t * this->M_c[c];
			// 计算 MtMc[c] 的逆矩阵并赋值给 MtMc_inv[c]
			MtMc_inv[c] = cConverter::invMat(MtMc[c]);
		}
		// 设置标志位 flagMcMt 为 true
		flagMcMt = true;
	}

	void cMultiCamSys_::Set_M_t(cv::Matx<double, 4, 4> M_t_)
	{
		// 将 M_t_ 转换为最小表示形式，使用 hom2cayley 方法
		M_t_min = hom2cayley<double>(M_t_);
		//M_t_min = hom2rodrigues<double>(M_t_);

		// 保存输入的 M_t_
		M_t = M_t_;

		// 计算 M_t_ 的逆矩阵
		M_t_inv = cConverter::invMat(M_t);

		// 遍历每个相机
		for (int c = 0; c < nrCams; ++c)
		{
			// 计算每个相机的变换矩阵 MtMc[c]
			MtMc[c] = M_t * this->M_c[c];
			// 计算每个相机的变换矩阵的逆矩阵 MtMc_inv[c]
			MtMc_inv[c] = cConverter::invMat(MtMc[c]);
		}

		// 设置标志位 flagMcMt 为 true
		flagMcMt = true;
	}

	// set calibration
	void cMultiCamSys_::Set_M_c_from_min(int c, cv::Matx<double, 6, 1> M_c_minRep)
	{
		// 将最小表示形式的相机矩阵M_c_minRep赋值给M_c_min数组的c位置
		M_c_min[c] = M_c_minRep;

		// 将最小表示形式的相机矩阵转换为齐次形式，并赋值给M_c数组的c位置
		M_c[c] = cayley2hom<double>(M_c_minRep);
		//M_c[c] = rodrigues2hom<double>(M_c_minRep);

		// 从齐次形式的相机矩阵M_c中提取旋转矩阵rTemp
		cv::Mat rTemp = cv::Mat(M_c[c])(cv::Rect(0, 0, 3, 3));

		// 从齐次形式的相机矩阵M_c中提取平移向量tTemp
		cv::Mat tTemp = cv::Mat(M_c[c])(cv::Rect(3, 0, 1, 3));

		// 将旋转矩阵转换为Eigen格式
		// to opengv
		Eigen::Matrix3d rotTemp;
		cv::cv2eigen<double>(rTemp, rotTemp);
		camRotations[c] = rotTemp;

		// 将平移向量转换为Eigen格式
		Eigen::Vector3d transTemp;
		cv::cv2eigen<double>(tTemp, transTemp);
		camOffsets[c] = transTemp;
	}

	void cMultiCamSys_::Set_M_c(int c, cv::Matx<double, 4, 4> M_c_)
	{
		// 将M_c_转换为Cayley表示法并存储在M_c_min[c]中
		M_c_min[c] = hom2cayley<double>(M_c_);
		// 注释掉将M_c_转换为Rodrigues表示法的代码
		//M_c_min[c] = hom2rodrigues<double>(M_c_);
		// 将M_c_直接存储在M_c[c]中
		M_c[c] = M_c_;

		// 从M_c_中提取旋转矩阵rTemp和平移向量tTemp
		cv::Mat rTemp = cv::Mat(M_c_)(cv::Rect(0, 0, 3, 3));
		cv::Mat tTemp = cv::Mat(M_c_)(cv::Rect(3, 0, 1, 3));
		// 转换为opengv格式
		// to opengv
		Eigen::Matrix3d rotTemp;
		cv::cv2eigen<double>(rTemp, rotTemp);
		camRotations[c] = rotTemp;

		Eigen::Vector3d transTemp;
		cv::cv2eigen<double>(tTemp, transTemp);
		camOffsets[c] = transTemp;
	}
	void cMultiCamSys_::Set_All_M_c(std::vector<cv::Matx<double, 4, 4>> M_c_)
	{
		// 将传入的M_c_赋值给成员变量M_c
		M_c = M_c_;

		// 遍历M_c中的每个元素
		for (int i = 0; i < M_c.size(); ++i)
		{
			// 将M_c中的每个元素转换为Cayley表示法，并添加到M_c_min中
			M_c_min.push_back(hom2cayley<double>(M_c[i]));
			//M_c_min.push_back(hom2rodrigues<double>(M_c[i]));

			// 从M_c中提取旋转矩阵rTemp
			cv::Mat rTemp = cv::Mat(M_c[i])(cv::Rect(0, 0, 3, 3));
			// 从M_c中提取平移向量tTemp
			cv::Mat tTemp = cv::Mat(M_c[i])(cv::Rect(3, 0, 1, 3));

			// to opengv
			// 创建一个Eigen的旋转矩阵rotTemp
			Eigen::Matrix3d rotTemp;
			// 将OpenCV的旋转矩阵转换为Eigen的旋转矩阵
			cv::cv2eigen<double>(rTemp, rotTemp);
			// 将Eigen的旋转矩阵添加到camRotations中
			camRotations.push_back(rotTemp);

			// 创建一个Eigen的平移向量transTemp
			Eigen::Vector3d transTemp;
			// 将OpenCV的平移向量转换为Eigen的平移向量
			cv::cv2eigen<double>(tTemp, transTemp);
			// 将Eigen的平移向量添加到camOffsets中
			camOffsets.push_back(transTemp);
		}
	}
	void cMultiCamSys_::Set_All_M_c_from_min(std::vector<cv::Matx<double, 6, 1>> M_c_min_)
	{
		// 将传入的 M_c_min_ 赋值给 M_c_min
		M_c_min = M_c_min_;

		// 遍历 M_c_min 中的每个元素
		for (int i = 0; i < M_c_min.size(); ++i)
		{
			// 将 M_c_min[i] 转换为 4x4 的齐次变换矩阵 c2h
			cv::Matx<double, 4, 4> c2h = cayley2hom<double>(M_c_min[i]);
			//cv::Matx<double, 4, 4> c2h = rodrigues2hom<double>(M_c_min[i]);

			// 将 c2h 添加到 M_c 中
			M_c.push_back(c2h);

			// 从 c2h 中提取旋转矩阵 rTemp
			cv::Mat rTemp = cv::Mat(c2h)(cv::Rect(0, 0, 3, 3));
			// 从 c2h 中提取平移向量 tTemp
			cv::Mat tTemp = cv::Mat(c2h)(cv::Rect(3, 0, 1, 3));

			// 将 rTemp 转换为 Eigen 矩阵 rotTemp
			Eigen::Matrix3d rotTemp;
			cv::cv2eigen<double>(rTemp, rotTemp);
			// 将 rotTemp 添加到 camRotations 中
			camRotations.push_back(rotTemp);

			// 将 tTemp 转换为 Eigen 向量 transTemp
			Eigen::Vector3d transTemp;
			cv::cv2eigen<double>(tTemp, transTemp);
			// 将 transTemp 添加到 camOffsets 中
			camOffsets.push_back(transTemp);
		}
	}

	// add MCS camera pose
	// and set the last one to the actual pose
	void cMultiCamSys_::Add_M_c(cv::Matx<double, 4, 4> M_c_)
	{
		// 将新的相机矩阵 M_c_ 添加到 M_c 列表中
		M_c.push_back(M_c_);

		// 将相机矩阵 M_c_ 转换为最小表示形式并添加到 M_c_min 列表中
		M_c_min.push_back(hom2cayley<double>(M_c_));
		//M_c_min.push_back(hom2rodrigues<double>(M_c_));

		// 从相机矩阵 M_c_ 中提取旋转矩阵 rTemp 和平移向量 tTemp
		cv::Mat rTemp = cv::Mat(M_c_)(cv::Rect(0, 0, 3, 3));
		cv::Mat tTemp = cv::Mat(M_c_)(cv::Rect(3, 0, 1, 3));

		// 将旋转矩阵 rTemp 和平移向量 tTemp 转换为 Eigen 格式
		// to opengv
		Eigen::Matrix3d rotTemp;
		cv::cv2eigen<double>(rTemp, rotTemp);
		camRotations.push_back(rotTemp);

		Eigen::Vector3d transTemp;
		cv::cv2eigen<double>(tTemp, transTemp);
		camOffsets.push_back(transTemp);

		// 增加相机数量
		nrCams++;
	}
	void cMultiCamSys_::Add_M_c_from_min(cv::Matx<double, 6, 1> M_c_min_)
	{
		M_c_min.push_back(M_c_min_);
		cv::Matx<double, 4, 4> c2h = cayley2hom(M_c_min_);
		//cv::Matx<double, 4, 4> c2h = rodrigues2hom(M_c_min_);
		M_c.push_back(c2h);

		cv::Mat rTemp = cv::Mat(c2h)(cv::Rect(0, 0, 3, 3));
		cv::Mat tTemp = cv::Mat(c2h)(cv::Rect(3, 0, 1, 3));

		// to opengv
		Eigen::Matrix3d rotTemp;
		cv::cv2eigen<double>(rTemp, rotTemp);
		camRotations.push_back(rotTemp);

		Eigen::Vector3d transTemp;
		cv::cv2eigen<double>(tTemp, transTemp);
		camOffsets.push_back(transTemp);

		nrCams++;
	}

	void cMultiCamSys_::Add_M_c_from_min_and_IO(cv::Matx<double, 6, 1> M_c_min_,
		cCamModelGeneral_ camM)
	{
		Add_M_c_from_min(M_c_min_);

		camModels.push_back(camM);

		//nrCams++; 
	}

	void cMultiCamSys_::Set_M_c_from_min_and_IO(int c, cv::Matx<double, 6, 1> M_c_min_, cCamModelGeneral_ camM)
	{
		M_c_min[c] = M_c_min_;
		M_c[c] = cayley2hom<double>(M_c_min_);
		//M_c[c] = rodrigues2hom<double>(M_c_min_);
		camModels[c] = camM;

		cv::Mat rTemp = cv::Mat(M_c[c])(cv::Rect(0, 0, 3, 3));
		cv::Mat tTemp = cv::Mat(M_c[c])(cv::Rect(3, 0, 1, 3));

		// to opengv
		Eigen::Matrix3d rotTemp;
		cv::cv2eigen<double>(rTemp, rotTemp);
		camRotations[c] = rotTemp;

		Eigen::Vector3d transTemp;
		cv::cv2eigen<double>(tTemp, transTemp);
		camOffsets[c] = transTemp;
	}
}